package com.suray.wcs.service.core.path.api.impl;

import com.suray.basic.wcs.path.core.Node;
import com.suray.basic.wcs.path.enums.NodeStatus;
import com.suray.basic.wcs.path.enums.NodeType;
import com.suray.basic.wcs.utils.*;
import com.suray.commin.exception.OrderExecuteException;
import com.suray.commin.exception.Rgv60TaskException;
import com.suray.rgv.RgvSMInterface;
import com.suray.wcs.res.enums.PalletStatus;
import com.suray.wcs.res.enums.RgvStatus;
import com.suray.wcs.service.core.path.api.impl.task.RgvSonTaskProcess;
import com.suray.wcs.service.core.util.LockNodeManager;
import com.suray.wcs.service.enums.TaskRgvProgress;
import com.suray.wcs.service.enums.TaskType;
import com.suray.wcs.service.pojo.Rgv;
import com.suray.wcs.service.pojo.RgvSM;
import com.suray.wcs.service.system.entity.TaskDB;
import com.suray.wcs.service.system.entity.WareHouseDB;
import com.suray.wcs.service.system.service.impl.NodeDBServiceImpl;
import com.suray.wcs.service.system.service.impl.TaskDBServiceImpl;
import com.suray.wcs.service.util.WcsUtil;
import org.apache.commons.lang3.ArrayUtils;

import java.io.IOException;
import java.util.*;

/**
 * 子车新任务模式
 */
public class RgvSonNewTaskHandle {

    private RgvSM rgv;
    private TaskDB taskDB;
    private Integer resTaskNo;
    private List<Node> pathNodes;
    private NodeDBServiceImpl nodeDBService;
    private TaskDBServiceImpl taskDBService;
    private boolean getCargoFlag = false;
    private boolean putCargoFlag = false;

    private int lastEndSegment;

    private RgvSonTaskProcess progressMap;

    private Node rgvNode;

    /**
     * 子车行驶：任务一次性下发
     * @param rgv
     * @param taskDB
     * @param resTaskNo
     * @param pathNodes
     * @param nodeDBService
     * @param taskdbService
     */
    public static void sonRgvNewRun(RgvSM rgv, TaskDB taskDB, Integer resTaskNo, Set<Coord> pathNodes,
                                    NodeDBServiceImpl nodeDBService, TaskDBServiceImpl taskdbService) {
        long time = System.currentTimeMillis();
        LoggerUtil.info("--等待母车行驶结束:TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(time)), RgvSonNewTaskHandle.class);
        waitRgvTaskEnd(rgv);
        long endTime = System.currentTimeMillis();
        LoggerUtil.info("--母车行驶结束：TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(endTime)) +
                ", 耗时:" + (endTime - time), RgvSonNewTaskHandle.class);
        lockNodesUtilOk(pathNodes, rgv);
        rgv.analyzeData();
        time = System.currentTimeMillis();
        LoggerUtil.info("--子车开始行驶：TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(time)) +
                ", 耗时:" + (time - endTime), RgvSonNewTaskHandle.class);
        new RgvSonNewTaskHandle(rgv, taskDB, resTaskNo, pathNodes, nodeDBService, taskdbService).start();
        endTime = System.currentTimeMillis();
        LoggerUtil.info("--子车行驶结束：TIME--" + WcsFuncUtil.DATE_FORMAT.format(new Date(endTime)) +
                ", 耗时:" + (endTime - time), RgvSonNewTaskHandle.class);
        if (rgv.isRgvSonInMother()) {
            unLockNodes(rgv, nodeDBService);
        }
    }

    /**
     * 子车行驶：任务一次性下发
     * @param rgv
     * @param taskDB
     * @param resTaskNo
     * @param pathNodes
     * @param rgvNode
     * @param nodeDBService
     * @param taskdbService
     */
    public static RgvSonNewTaskHandle getRgvSonTaskHandle(RgvSM rgv, TaskDB taskDB, Integer resTaskNo,
                                                          Set<Coord> pathNodes, Node rgvNode,
                                                          NodeDBServiceImpl nodeDBService,
                                                          TaskDBServiceImpl taskdbService) {
        if (!lockNodes(pathNodes, rgv)) {
            return null;
        }
        rgv.analyzeData();
        return new RgvSonNewTaskHandle(rgv, taskDB, resTaskNo, pathNodes, rgvNode, nodeDBService, taskdbService);
    }

    /**
     * 点位上锁
     * @param nodeList
     * @param rgv
     */
    public static boolean lockNodes(Set<Coord> nodeList, RgvSM rgv) {
        Set<Coord> nodes = new HashSet<>(nodeList);
        nodes.remove(rgv.getRgvDB().getLocationNode());
        LoggerUtil.info(ModuleType.RGV, LoggerType.INFO, rgv.getRgvName(), "锁定子车行驶节点",
                WcsFuncUtil.listToString(nodes), RgvSonNewTaskHandle.class);
        return LockNodeManager.lockedNodesSafe(nodes, rgv);
    }

    /**
     * 点位解锁
     * @param rgv
     * @param nodeDBService
     */
    public static void unLockNodes(RgvSM rgv, NodeDBServiceImpl nodeDBService) {
        RgvSMInterface rgvSMInterface = rgv.getRgvCore();
        Set<Coord> coords = LockNodeManager.getLockCoordByObj(rgv);
        coords.remove(rgv.getCurLocation());
        if (!rgvSMInterface.isSonRgvInMother()) {
            coords.remove(nodeDBService.getNodeByGroundCode(rgvSMInterface.getSonRgvGroundCode()));
        }
        LockNodeManager.unlockedNodes(coords, rgv);
    }

    /**
     * 点位上锁, 知道锁定成功
     * @param nodeList
     * @param rgv
     */
    public static void lockNodesUtilOk(Set<Coord> nodeList, RgvSM rgv) {
        long time = System.currentTimeMillis();
        while (true) {
            rgv.checkInterrupt("锁定路径", time);
            if (lockNodes(nodeList, rgv)) {
                return;
            }

            WcsFuncUtil.delay(1000);
        }
    }

    private static void waitRgvTaskEnd(RgvSM rgv) {
        long time = System.currentTimeMillis();
        while (rgv.getRgvCore().hasTask()) {
            rgv.checkInterrupt("等待母车任务执行结束", time);
            LoggerUtil.info(rgv.getRgvName() + " 母车有任务未完成， 子车不允许执行任务", RgvSonNewTaskHandle.class);
            WcsFuncUtil.delay(1000);
        }
    }

    private RgvSonNewTaskHandle(RgvSM rgv, TaskDB taskDB, Integer resTaskNo, Set<Coord> pathNodes,
                                NodeDBServiceImpl nodeDBService, TaskDBServiceImpl taskDBService) {
        this.rgv = rgv;
        this.taskDB = taskDB;
        this.resTaskNo = resTaskNo;

        this.nodeDBService = nodeDBService;
        this.taskDBService = taskDBService;

        this.rgvNode = rgv.getCurLocation();
        analyzeTaskBeforeRun(rgv, taskDB, taskDBService);
        this.pathNodes = calcPath(rgv, taskDB, pathNodes);

        //母车所在位置条码改为母车条码
        if (this.pathNodes.contains(rgvNode)) {
            this.pathNodes.get(this.pathNodes.indexOf(rgvNode))
                    .setSonRgvGroundCode(rgv.getRgvCore().getSonGroundCodeInMother());
        }

        this.lastEndSegment = -1;
    }

    private RgvSonNewTaskHandle(RgvSM rgv, TaskDB taskDB, Integer resTaskNo, Set<Coord> pathNodes,
                                Node rgvNode, NodeDBServiceImpl nodeDBService, TaskDBServiceImpl taskDBService) {
        this.rgv = rgv;
        this.taskDB = taskDB;
        this.resTaskNo = resTaskNo;

        this.nodeDBService = nodeDBService;
        this.taskDBService = taskDBService;

        this.rgvNode = rgvNode;

//        analyzeTaskBeforeRun(rgv, taskDB, taskDBService);
        this.pathNodes = calcPath(rgv, taskDB, pathNodes);

        //母车所在位置条码改为母车条码
        this.pathNodes.get(this.pathNodes.indexOf(rgvNode))
                .setSonRgvGroundCode(rgv.getRgvCore().getSonGroundCodeInMother());

        this.lastEndSegment = -1;
    }

    /**
     * 计算车行驶的路径
     * @param rgv
     * @param taskDB
     * @param pathNodes
     */
    private List<Node> calcPath(RgvSM rgv, TaskDB taskDB, Set<Coord> pathNodes) {
        switch (taskDB.getRgvProgress()) {
            case RGV_SON_LEAVE_GET:
                if (rgv.isRgvSonInMother()) {
                    return new ArrayList<>();
                }
            case RGV_TO_START:
            case RGV_SON_OUT_TO_GET:
            case RGV_GET_CARGO:
                if (checkIfSameRoadway(taskDB, nodeDBService)) {
                    //同巷道，返回路径长的
                    List<Node> startPath = nodeDBService.findNodes(analyzeLineNodes(rgvNode,
                            taskDB.selectStartNode()));
                    List<Node> endPath = nodeDBService.findNodes(analyzeLineNodes(rgvNode,
                            taskDB.selectEndNode()));
                    return startPath.size() > endPath.size() ? startPath : endPath;
                } else {
                    if (taskDB.getRgvProgress() == TaskRgvProgress.RGV_GET_CARGO) {
                        if (rgv.isRgvSonInMother()) {
                            return new ArrayList<>();
                        }
                    }

                    return nodeDBService.findNodes(analyzeLineNodes(rgvNode, taskDB.selectStartNode()));
                }
            case RGV_SON_LEAVE_PUT:
            case RGV_PUT_CARGO:
                if (rgv.isRgvSonInMother()) {
                    return new ArrayList<>();
                }
            case RGV_TO_END:
            case RGV_SON_OUT_TO_PUT:
                return nodeDBService.findNodes(analyzeLineNodes(rgvNode, taskDB.selectEndNode()));
            case RGV_SON_GOTO_TARGET:
                return nodeDBService.findNodes(pathNodes);
            default:
                return new ArrayList<>();
        }
    }

    public void start() {
        if (pathNodes.size() == 0) {
            LoggerUtil.info(rgv.getRgvName() + " 子车无需行驶:" + taskDB, this.getClass());
            return;
        }

        doTask();
        waitRgvSonTaskEnd(rgv.getRgvCore());
    }

    public void doTask() {
        LoggerUtil.info(rgv.getRgvName() + " 子车开始执行任务:" + taskDB, this.getClass());
        RgvSMInterface rgvSM = rgv.getRgvCore();
        if (rgvSM.isSonRgvTaskError()) {
            throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), rgvSM.getSonRgvTaskErrorCode());
        }
        checkPathNodesIsLine(pathNodes);
        checkSonRgvCanRun(rgv, taskDB);
        rgvSonTaskRun(rgvSM);
    }

    public void sendTask() {
        LoggerUtil.info(rgv.getRgvName() + " 子车开始执行任务:" + taskDB, this.getClass());
        checkPathNodesIsLine(pathNodes);
        checkSonRgvCanRun(rgv, taskDB);
        rgvSonTaskNotRun(rgv.getRgvCore());
    }

    /**
     * 检测起点与终点间是否有货，不包含起点，包含终点
     * @param start
     * @param end
     */
    private boolean checkIfHasCargo(Node start, Node end, List<Node> nodes) {
        int startIndex = nodes.indexOf(start);
        int endIndex = nodes.indexOf(end);

        if (startIndex == -1 || endIndex == -1) {
            throw new RuntimeException("路径中找不到位置：" + WcsFuncUtil.listToString(nodes) + ", 起点:" + start +
                    ", 终点:" + end);
        }

        if (startIndex < endIndex) {
            for (int i = startIndex + 1; i <= endIndex; i++) {
                if (nodes.get(i).getNodeStatus() == NodeStatus.Y) {
                    return true;
                }
            }
        } else if (startIndex > endIndex) {
            for (int i = startIndex - 1; i >= endIndex; i--) {
                if (nodes.get(i).getNodeStatus() == NodeStatus.Y) {
                    return true;
                }
            }
        }

        return false;
    }

    /**
     * 检测子车状态是否允许执行任务
     * @param rgv
     * @param taskDB
     */
    private void checkSonRgvCanRun(RgvSM rgv, TaskDB taskDB) {
        TaskRgvProgress rgvProgress = taskDB.getRgvProgress();
        if (taskDB.getTaskType() == TaskType.MOVE_UNLOADED || taskDB.getTaskType() == TaskType.RGV_SON_MOVE_UNLOADED
            || taskDB.getTaskType() == TaskType.MOVE_LOADED) {
            rgvProgress = TaskRgvProgress.RGV_SON_GOTO_TARGET;
        } else {
            if (rgvProgress == TaskRgvProgress.RGV_TO_START) {
                rgvProgress = TaskRgvProgress.RGV_SON_OUT_TO_GET;
            } else if (rgvProgress == TaskRgvProgress.RGV_TO_END) {
                rgvProgress = TaskRgvProgress.RGV_SON_OUT_TO_PUT;
            }
        }

        switch (rgvProgress) {
            case RGV_SON_LEAVE_PUT:
                checkSonRgvCanRun(rgv, pathNodes, nodeDBService, PalletStatus.DOWN);
                break;
            case RGV_SON_OUT_TO_GET:
                checkSonRgvCanRun(rgv, pathNodes, nodeDBService, PalletStatus.DOWN);
                checkIfHasCargo(taskDB.selectStartNode(), rgvNode);
                break;
            case RGV_SON_LEAVE_GET:
                checkSonRgvCanRun(rgv, pathNodes, nodeDBService, PalletStatus.UP);
                checkIfHasCargo(rgv.getSonRgvLocation(), rgv.getCurLocation());
                break;
            case RGV_SON_OUT_TO_PUT:
                checkSonRgvCanRun(rgv, pathNodes, nodeDBService, PalletStatus.UP);
                checkIfHasCargo(rgvNode, taskDB.selectEndNode());
                break;
            case RGV_GET_CARGO:
            case RGV_PUT_CARGO:
                break;
            case RGV_SON_GOTO_TARGET:
                checkSonRgvCanRun(rgv, pathNodes, nodeDBService, rgv.getPalletStatus());
                break;
            default:
                throw new RuntimeException("任务进度错误，无法执行子车任务:" + taskDB.getRgvProgress());
        }
    }

    private void checkIfHasCargo(Node startNode, Node endNode) {
        if (checkIfSameRoadway(taskDB, nodeDBService)) {
            if (checkIfHasCargo(taskDB.selectStartNode(), taskDB.selectEndNode(), pathNodes)) {
                throw new RuntimeException("起点到终点位置有货：" + taskDB.selectStartNode() + "->"
                        + taskDB.selectEndNode() + ", 无法移货");
            }
        } else {
            if (checkIfHasCargo(startNode, endNode, pathNodes)) {
                throw new RuntimeException("起点到终点位置有货：" + taskDB.selectStartNode() + "->"
                        + taskDB.selectEndNode() + ", 无法移货");
            }
        }
    }

    private void rgvSonTaskRun(RgvSMInterface rgvSM) {
        progressMap = generalProcessMap();
        rgvSonTaskSend(rgvSM, progressMap, true);
    }

    private void rgvSonTaskNotRun(RgvSMInterface rgvSM) {
        progressMap = generalProcessMap();
        rgvSonTaskSend(rgvSM, progressMap, false);
    }

    private RgvSonTaskProcess generalProcessMap() {
        return analyzeRgvSonTask(rgv, pathNodes, taskDB);
    }

    private void rgvSonTaskSend(RgvSMInterface rgvSM, RgvSonTaskProcess progressMap, boolean startTask) {
        int[] pathIndexs = ArrayUtils.toPrimitive(progressMap.getPathIndexs().toArray(new Integer[0]));
        long time = System.currentTimeMillis();
        while (rgvSM.getSonRgvStatus() != RgvStatus.READY) {
            rgv.checkInterrupt("等待子车就绪", time);
            WcsFuncUtil.delay(100);
        }
        while (true) {
            try {
                rgv.checkInterrupt("下发子车任务", time);
                rgvSM.sonRgvSendTask(taskDB.getTaskId(), resTaskNo, pathNodes, pathIndexs, startTask);
                break;
            } catch (IOException e) {
                LoggerUtil.warn(ModuleType.RGV_SON_MOTHER, LoggerType.INFO, taskDB.getTaskId(), rgv.getRgvName(),
                        "下发子车任务时通讯失败", this.getClass());
                WcsFuncUtil.delay(1000);
            }
        }
        LoggerUtil.info(rgv.getRgvName() + " 等待子车任务执行完成:" + resTaskNo + ", 母车任务ID:" + taskDB.getTaskId(),
                this.getClass());
    }

    public void waitRgvSonTaskEnd(RgvSMInterface rgvSM) {
        long time = System.currentTimeMillis();
        boolean flag = false;
        while (!isRgvTaskEnd(rgvSM, taskDB, progressMap, time)) {
            rgv.checkInterrupt("等待子车任务完成:waitRgvSonTaskEnd", time);

            if (!flag) {
                if (rgvSM.getSonRgvStatus() != RgvStatus.READY) {
                    LoggerUtil.info("子车开始执行任务：TIME--"
                            + WcsFuncUtil.DATE_FORMAT.format(new Date(System.currentTimeMillis())), this.getClass());
                    flag = true;
                }
            }

            if (rgvSM.isSonRgvTaskError()) {
                throw new OrderExecuteException(rgv.getRgvDB().getRgvNo(), rgvSM.getSonRgvTaskErrorCode());
            }

            WcsFuncUtil.delay(200);
        }
        if (rgv.isRgvSonInMother()) {
            unLockNodes(rgv, nodeDBService);
        }
        LoggerUtil.info(rgv.getRgvName() + " 子车任务执行完成:" + resTaskNo + ", 母车任务ID:" + taskDB.getTaskId(),
                this.getClass());
    }

    private boolean isRgvTaskEnd(RgvSMInterface rgvSM, TaskDB taskDB, RgvSonTaskProcess progressMap, long time) {
        int endSegment = rgvSM.getRgvSonEndSegment(resTaskNo, time);
        int processIndex = progressMap.getSegments().indexOf(endSegment);
        if (lastEndSegment < processIndex) {
            for (int i = processIndex; i > lastEndSegment; i--) {
                TaskRgvProgress rgvProgress = progressMap.getProgresses().get(i);
                if (rgvProgress == TaskRgvProgress.RGV_GET_CARGO) {
                    WcsUtil.changeNodeCargoStatus(taskDB.selectStartNode(), nodeDBService, false);
                    getCargoFlag = true;
                } else if (rgvProgress == TaskRgvProgress.RGV_PUT_CARGO) {
                    WcsUtil.changeNodeCargoStatus(taskDB.selectEndNode(), nodeDBService, true);
                    putCargoFlag = true;
                }

                taskDBService.updateTaskRgvProgress(taskDB, rgvProgress);
            }

            lastEndSegment = processIndex;
        }

        if (rgvSM.isRgvSonTaskEnd(taskDB.getTaskId(), resTaskNo, time)) {
            if (taskDB.getTaskType() == TaskType.MOVE_UNLOADED || taskDB.getTaskType() == TaskType.RGV_SON_MOVE_UNLOADED
                    || taskDB.getTaskType() == TaskType.MOVE_LOADED) {
                return true;
            } else {
                if (progressMap.getProgresses().get(processIndex) == TaskRgvProgress.RGV_SON_LEAVE_PUT
                        || progressMap.getProgresses().get(processIndex) == TaskRgvProgress.RGV_SON_LEAVE_GET) {
                    TaskType taskType = taskDB.getTaskType();
                    if (taskType == TaskType.TRANSFER || taskType == TaskType.OUT || taskType == TaskType.IN) {
                        List<TaskRgvProgress> progresses = progressMap.getProgresses();
                        if (progresses.contains(TaskRgvProgress.RGV_GET_CARGO) && !getCargoFlag) {
                            WcsUtil.changeNodeCargoStatus(taskDB.selectStartNode(), nodeDBService, false);
                        }
                        if (progresses.contains(TaskRgvProgress.RGV_PUT_CARGO) && !putCargoFlag) {
                            WcsUtil.changeNodeCargoStatus(taskDB.selectEndNode(), nodeDBService, true);
                        }
                    }

                    if (progressMap.getProgresses().get(processIndex) == TaskRgvProgress.RGV_SON_LEAVE_PUT) {
                        taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_COMPLETE);
                    } else if (progressMap.getProgresses().get(processIndex) == TaskRgvProgress.RGV_SON_LEAVE_GET) {
                        taskDBService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_TO_END);
                    }
                    return true;
                }
            }
        }

        //子车行驶时，位置返回不正确，不能用来判断母车重启
        if (!rgvSM.hasAuthor()/* || !rgvSM.positionOk()*/) {
            throw new RuntimeException("母车可能被重启，任务进度可能丢失，请人为确认子车任务执行进度后，修改当前的任务进度和货位状态");
        }
        return false;
    }

    /**
     * 解析段序号与任务进度
     * @param rgv
     * @param nodes : 整体的路径
     * @param taskDB
     */
    public RgvSonTaskProcess analyzeRgvSonTask(RgvSM rgv, List<Node> nodes, TaskDB taskDB) {
        RgvSonTaskProcess progressMap = new RgvSonTaskProcess();
        boolean sameRoadway = checkIfSameRoadway(taskDB, nodeDBService);
        int segment = 0;

        TaskRgvProgress rgvProgress = taskDB.getRgvProgress();
        if (taskDB.getTaskType() == TaskType.MOVE_UNLOADED || taskDB.getTaskType() == TaskType.RGV_SON_MOVE_UNLOADED
                || taskDB.getTaskType() == TaskType.MOVE_LOADED) {
            rgvProgress = TaskRgvProgress.RGV_SON_GOTO_TARGET;
        } else {
            if (rgvProgress == TaskRgvProgress.RGV_TO_START) {
                rgvProgress = TaskRgvProgress.RGV_SON_OUT_TO_GET;
            } else if (rgvProgress == TaskRgvProgress.RGV_TO_END) {
                rgvProgress = TaskRgvProgress.RGV_SON_OUT_TO_PUT;
            }
        }
        switch (rgvProgress) {
            case RGV_SON_OUT_TO_GET:
                int indexGetCargo = nodes.indexOf(taskDB.selectStartNode());
                nodes.get(indexGetCargo).setResAction(Action.PALLET_UP);
                if (!rgv.getSonRgvLocation().equals(taskDB.selectStartNode())) {
                    segment++;
                }
                progressMap.put(indexGetCargo, TaskRgvProgress.RGV_GET_CARGO, segment);
            case RGV_GET_CARGO:
            case RGV_SON_LEAVE_GET:
                if (!sameRoadway) {
                    progressMap.put(nodes.indexOf(rgvNode), TaskRgvProgress.RGV_SON_LEAVE_GET, ++segment);
                    break;
                }
            case RGV_SON_OUT_TO_PUT:
                int indexPutCargo = nodes.indexOf(taskDB.selectEndNode());
                nodes.get(indexPutCargo).setResAction(Action.PALLET_DOWN);
                if (taskDB.getRgvProgress() == TaskRgvProgress.RGV_SON_OUT_TO_PUT
                        || taskDB.getRgvProgress() == TaskRgvProgress.RGV_GET_CARGO) {
                    if (!rgv.getSonRgvLocation().equals(taskDB.selectEndNode())) {
                        segment++;
                    }
                } else {
                    segment++;
                }
                progressMap.put(indexPutCargo, TaskRgvProgress.RGV_PUT_CARGO, segment);
            case RGV_PUT_CARGO:
            case RGV_SON_LEAVE_PUT:
                progressMap.put(nodes.indexOf(rgvNode), TaskRgvProgress.RGV_SON_LEAVE_PUT, ++segment);
                break;
            case RGV_SON_GOTO_TARGET:
                progressMap.put(nodes.indexOf(taskDB.selectEndNode()), TaskRgvProgress.RGV_SON_GOTO_TARGET, ++segment);
                break;
            default:
                throw new RuntimeException("任务进度错误，无法执行子车任务");
        }

        return progressMap;
    }

    /**
     * 点位是否在库区内
     * @param node
     * @param wareHouseDB
     * @return
     */
    public static boolean inWareHouse(Node node, WareHouseDB wareHouseDB) {
        int minX = wareHouseDB.getMinX();
        int minY = wareHouseDB.getMinY();
        int maxX = wareHouseDB.getMaxX();
        int maxY = wareHouseDB.getMaxY();

        if (node.getX() >= minX && node.getX() <= maxX && node.getY() >= minY && node.getY() <= maxY) {
            return true;
        }

        return false;
    }
    /**
     * 检测路径节点是否在一条直线上
     * @param pathNodes
     */
    public static void checkPathNodesIsLine(List<Node> pathNodes) {
        if (WcsFuncUtil.isNullList(pathNodes)) {
            return;
        }

        Node nodePre = pathNodes.get(0);
        Node nodeAft = pathNodes.get(1);
        if (nodePre.getX() == nodeAft.getX()) {
            for (int i = 2; i < pathNodes.size(); i++) {
                nodePre = nodeAft;
                nodeAft = pathNodes.get(i);
                if (nodePre.getX() != nodeAft.getX()) {
                    throw new Rgv60TaskException("子车任务节点不在一条直线上：" + WcsFuncUtil.listToString(pathNodes));
                }
            }
        } else if (nodePre.getY() == nodeAft.getY()) {
            for (int i = 2; i < pathNodes.size(); i++) {
                nodePre = nodeAft;
                nodeAft = pathNodes.get(i);
                if (nodePre.getY() != nodeAft.getY()) {
                    throw new Rgv60TaskException("子车任务节点不在一条直线上：" + WcsFuncUtil.listToString(pathNodes));
                }
            }
        } else {
            throw new Rgv60TaskException("子车任务节点不在一条直线上：" + WcsFuncUtil.listToString(pathNodes));
        }
    }

    /**
     * 检测任务的起点与终点是否是同巷道
     * taskDB 必须为移库任务
     * @param taskDB
     * @param nodeDBService
     * @return
     */
    public static boolean checkIfSameRoadway(TaskDB taskDB, NodeDBServiceImpl nodeDBService) {
        try {
            Node startNode = taskDB.selectStartNode();
            Node endNode = taskDB.selectEndNode();

            if (startNode.getZ() != endNode.getZ()) {
                return false;
            }

            Set<Coord> coords = analyzeLineNodes(startNode, endNode);
            List<Node> nodes = nodeDBService.findNodes(coords);
            for (int i = 1; i < nodes.size(); i++) {
                Node node = nodes.get(i);
                Node preNode = nodes.get(i - 1);
                if (node.getX() == preNode.getX()) {
                    if (node.getY() > preNode.getY() && !preNode.getRight()) {
                        return false;
                    } else if (node.getY() < preNode.getY() && !preNode.getLeft()) {
                        return false;
                    }
                } else if (node.getY() == preNode.getY()) {
                    if (node.getX() > preNode.getX() && !preNode.getDown()) {
                        return false;
                    } else if (node.getX() < preNode.getX() && !preNode.getUp()) {
                        return false;
                    }
                } else {
                    return false;
                }

                if (!node.getNodeType().equals(NodeType.P)) {
                    return false;
                }
            }
        } catch (RuntimeException e) {
            return false;
        }

        LoggerUtil.info("同巷道移库：" + taskDB, RgvSonNewTaskHandle.class);
        return true;
    }

    /**
     * 解析两点间的节点
     * 两点必须在一条直线上
     * @param startNode
     * @param endNode
     * @return
     */
    public static Set<Coord> analyzeLineNodes(Node startNode, Node endNode) {
        Set<Coord> lineNodes = new LinkedHashSet<>();
        if (startNode.equals(endNode)) {
            return lineNodes;
        }
        Node node = new Node(startNode);
        lineNodes.add(node);
        int offset = 1;
        if (startNode.getX() == endNode.getX()) {
            if (endNode.getY() < startNode.getY()) {
                offset = -1;
            }
            while (node.getY() != endNode.getY()) {
                node = new Node(node.getX(), node.getY() + offset, node.getZ());
                lineNodes.add(node);
            }
        } else if (startNode.getY() == endNode.getY()) {
            if (endNode.getX() < startNode.getX()) {
                offset = -1;
            }
            while (node.getX() != endNode.getX()) {
                node = new Node(node.getX() + offset, node.getY(), node.getZ());
                lineNodes.add(node);
            }
        } else {
            throw new Rgv60TaskException("两点不在一条直线上：" + startNode + endNode);
        }

        return lineNodes;
    }

    /**
     * 如果子车顶升，检测路径上是否有货
     * 如果托盘实际状态与理论的状态不一致，则报错
     * @param rgv
     * @param nodes
     * @param nodeDBService
     * @param palletStatus : 当前托盘理论上的状态
     */
    private void checkSonRgvCanRun(RgvSM rgv, List<Node> nodes, NodeDBServiceImpl nodeDBService,
                                   PalletStatus palletStatus) {
        checkPathNodesIsLine(nodes);

        if (rgv.getPalletStatus() != palletStatus) {
            throw new Rgv60TaskException(rgv.getRgvName() + "当前的托盘状态：" + rgv.getPalletStatus() +
                    ", 当前任务需要的托盘状态：" + palletStatus + ", 不允许执行任务");
        }

        if (rgv.getPalletStatus() == PalletStatus.UP) {
            Node sonNode = getSonRgvNode(rgv, nodeDBService);
            for (Node node : nodes) {
                if (node.equals(sonNode)) {
                    continue;
                } else if (node.getNodeStatus() == NodeStatus.Y) {
                    throw new Rgv60TaskException("子车托盘顶升，子车任务经过点有货:" + node);
                }
            }
        } else if (palletStatus == PalletStatus.DOWN) {
            //托盘为下降状态，可以自由移动
        } else {
            throw new Rgv60TaskException("托盘状态异常, 不能执行任务");
        }

        LoggerUtil.info(ModuleType.RGV, LoggerType.INFO, rgv.getRgvName(), "子车行驶节点",
                WcsFuncUtil.listToString(nodes), this.getClass());
    }

    /**
     * 获取子车坐标
     * @param rgv
     * @param nodeDBService
     * @return
     */
    public Node getSonRgvNode(RgvSM rgv, NodeDBServiceImpl nodeDBService) {
        if (rgv.isRgvSonInMother()) {
            return rgvNode;
        } else {
            return nodeDBService.getNodeByGroundCode(rgv.getRgvCore().getSonRgvGroundCode());
        }
    }

    public void analyzeTaskBeforeRun(Rgv rgv, TaskDB taskDB, TaskDBServiceImpl taskdbService) {
        if (taskDB.getTaskType() == TaskType.RGV_SON_MOVE_UNLOADED || taskDB.getTaskType() == TaskType.MOVE_LOADED) {
            taskdbService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_SON_GOTO_TARGET);
        } else {
            TaskRgvProgress progress = taskDB.getRgvProgress();
            if (progress == TaskRgvProgress.RGV_TO_START) {
                taskdbService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_SON_OUT_TO_GET);
            } else if (progress == TaskRgvProgress.RGV_TO_END) {
                if (taskDB.getTaskType() == TaskType.MOVE_UNLOADED) {
                    taskdbService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_SON_GOTO_TARGET);
                } else {
                    taskdbService.updateTaskRgvProgress(taskDB, TaskRgvProgress.RGV_SON_OUT_TO_PUT);
                }
            }
        }

        LoggerUtil.info(ModuleType.RGV, LoggerType.INFO, rgv.getRgvName(), "当前子车任务进度：",
                taskDB.getRgvProgress().getDescription(), this.getClass());
    }

    public void startTask() throws IOException {
        analyzeTaskBeforeRun(rgv, taskDB, taskDBService);
        rgv.getRgvCore().sonRgvTaskStart();
        waitRgvSonTaskEnd(rgv.getRgvCore());
    }
}
